from __future__ import division
from sympy import symbols, sqrt
from sympy.simplify import ratsimp
from sympy.simplify.cse_main import cse
from math import acos
from numpy import matrix, float64
from scipy.linalg import solve, lstsq

def hess1(pt1, pt2, pt3, pt4):
    x1, y1, z1 = symbols('x1 y1 z1', real=True)
    x2, y2, z2 = symbols('x2 y2 z2', real=True)
    xa, ya, za = symbols('xa ya za', real=True)

    nx1 = y1 * za - z1 * ya
    ny1 = z1 * xa - x1 * za
    nz1 = x1 * ya - y1 * xa

    nx2 = y2 * za - z2 * ya
    ny2 = z2 * xa - x2 * za
    nz2 = x2 * ya - y2 * xa

    vn1xvaX = ny1*za - nz1*ya
    vn1xvaY = nz1*xa - nx1*za
    vn1xvaZ = nx1*ya - ny1*xa

    vn1xv1X = ny1*z1 - nz1*y1
    vn1xv1Y = nz1*x1 - nx1*z1
    vn1xv1Z = nx1*y1 - ny1*x1

    vn1xv2X = ny1*z2 - nz1*y2
    vn1xv2Y = nz1*x2 - nx1*z2
    vn1xv2Z = nx1*y2 - ny1*x2

    vn2xvaX = ny2*za - nz2*ya
    vn2xvaY = nz2*xa - nx2*za
    vn2xvaZ = nx2*ya - ny2*xa

    vn2xv2X = ny2*z2 - nz2*y2
    vn2xv2Y = nz2*x2 - nx2*z2
    vn2xv2Z = nx2*y2 - ny2*x2

    vn2xv1X = ny2*z1 - nz2*y1
    vn2xv1Y = nz2*x1 - nx2*z1
    vn2xv1Z = nx2*y1 - ny2*x1


    r1 = sqrt(nx1*nx1+ny1*ny1+nz1*nz1)
    r2 = sqrt(nx2*nx2+ny2*ny2+nz2*nz2)
    r12 = sqrt((nx1*nx1 + ny1*ny1 + nz1*nz1)*(nx2*nx2 + ny2*ny2 + nz2*nz2))

    dotN12 = (nx1*nx2+ny1*ny2+nz1*nz2)
    cost = dotN12/sqrt((nx1*nx1+ny1*ny1+nz1*nz1)*(nx2*nx2+ny2*ny2+nz2*nz2))

    data = {'x1': pt1.x-pt2.x, 'y1':pt1.y-pt2.y, 'z1':pt1.z-pt2.z,
            'x2': pt4.x-pt3.x, 'y2':pt4.y-pt3.y, 'z2':pt4.z-pt3.z,
            'xa': pt2.x-pt3.x, 'ya':pt2.y-pt3.y, 'za':pt2.z-pt3.z,}

    return [
        [   cost.diff(x1, x1)(data), cost.diff(x1, y1)(data), cost.diff(x1, z1)(data),
            cost.diff(y1, x1)(data), cost.diff(y1, y1)(data), cost.diff(y1, z1)(data),
            cost.diff(z1, x1)(data), cost.diff(z1, y1)(data), cost.diff(z1, z1)(data),],
            
        [   cost.diff(x1, x2)(data), cost.diff(x1, y2)(data), cost.diff(x1, z2)(data),
            cost.diff(y1, x2)(data), cost.diff(y1, y2)(data), cost.diff(y1, z2)(data),
            cost.diff(z1, x2)(data), cost.diff(z1, y2)(data), cost.diff(z1, z2)(data),],

        [   cost.diff(x1, xa)(data), cost.diff(x1, ya)(data), cost.diff(x1, za)(data),
            cost.diff(y1, xa)(data), cost.diff(y1, ya)(data), cost.diff(y1, za)(data),
            cost.diff(z1, xa)(data), cost.diff(z1, ya)(data), cost.diff(z1, za)(data),],
        
        [   cost.diff(x2, x1)(data), cost.diff(x2, y1)(data), cost.diff(x2, z1)(data),
            cost.diff(y2, x1)(data), cost.diff(y2, y1)(data), cost.diff(y2, z1)(data),
            cost.diff(z2, x1)(data), cost.diff(z2, y1)(data), cost.diff(z2, z1)(data),],
            
        [   cost.diff(x2, x2)(data), cost.diff(x2, y2)(data), cost.diff(x2, z2)(data),
            cost.diff(y2, x2)(data), cost.diff(y2, y2)(data), cost.diff(y2, z2)(data),
            cost.diff(z2, x2)(data), cost.diff(z2, y2)(data), cost.diff(z2, z2)(data),],
            
        [   cost.diff(x2, xa)(data), cost.diff(x2, ya)(data), cost.diff(x2, za)(data),
            cost.diff(y2, xa)(data), cost.diff(y2, ya)(data), cost.diff(y2, za)(data),
            cost.diff(z2, xa)(data), cost.diff(z2, ya)(data), cost.diff(z2, za)(data),],

        [   cost.diff(xa, x1)(data), cost.diff(xa, y1)(data), cost.diff(xa, z1)(data),
            cost.diff(ya, x1)(data), cost.diff(ya, y1)(data), cost.diff(ya, z1)(data),
            cost.diff(za, x1)(data), cost.diff(za, y1)(data), cost.diff(za, z1)(data),],
            
        [   cost.diff(xa, x2)(data), cost.diff(xa, y2)(data), cost.diff(xa, z2)(data),
            cost.diff(ya, x2)(data), cost.diff(ya, y2)(data), cost.diff(ya, z2)(data),
            cost.diff(za, x2)(data), cost.diff(za, y2)(data), cost.diff(za, z2)(data),],
        
        [   cost.diff(xa, xa)(data), cost.diff(xa, ya)(data), cost.diff(xa, za)(data),
            cost.diff(ya, xa)(data), cost.diff(ya, ya)(data), cost.diff(ya, za)(data),
            cost.diff(za, xa)(data), cost.diff(za, ya)(data), cost.diff(za, za)(data),],
    ]
    
def hess2(pt1, pt2, pt3, pt4):
    vx1, vy1, vz1 = symbols('x1 y1 z1', real=True)
    vx2, vy2, vz2 = symbols('x2 y2 z2', real=True)
    vx3, vy3, vz3 = symbols('x3 y3 z3', real=True)
    vx4, vy4, vz4 = symbols('x4 y4 z4', real=True)
    
    xa = vx2-vx3
    ya = vy2-vy3
    za = vz2-vz3
    x1 = vx1-vx2
    y1 = vy1-vy2
    z1 = vz1-vz2
    x2 = vx4-vx3
    y2 = vy4-vy3
    z2 = vz4-vz3

    nx1 = y1 * za - z1 * ya
    ny1 = z1 * xa - x1 * za
    nz1 = x1 * ya - y1 * xa

    nx2 = y2 * za - z2 * ya
    ny2 = z2 * xa - x2 * za
    nz2 = x2 * ya - y2 * xa

    vn1xvaX = ny1*za - nz1*ya
    vn1xvaY = nz1*xa - nx1*za
    vn1xvaZ = nx1*ya - ny1*xa

    vn1xv1X = ny1*z1 - nz1*y1
    vn1xv1Y = nz1*x1 - nx1*z1
    vn1xv1Z = nx1*y1 - ny1*x1

    vn1xv2X = ny1*z2 - nz1*y2
    vn1xv2Y = nz1*x2 - nx1*z2
    vn1xv2Z = nx1*y2 - ny1*x2

    vn2xvaX = ny2*za - nz2*ya
    vn2xvaY = nz2*xa - nx2*za
    vn2xvaZ = nx2*ya - ny2*xa

    vn2xv2X = ny2*z2 - nz2*y2
    vn2xv2Y = nz2*x2 - nx2*z2
    vn2xv2Z = nx2*y2 - ny2*x2

    vn2xv1X = ny2*z1 - nz2*y1
    vn2xv1Y = nz2*x1 - nx2*z1
    vn2xv1Z = nx2*y1 - ny2*x1


    r1 = sqrt(nx1*nx1+ny1*ny1+nz1*nz1)
    r2 = sqrt(nx2*nx2+ny2*ny2+nz2*nz2)
    r12 = sqrt((nx1*nx1 + ny1*ny1 + nz1*nz1)*(nx2*nx2 + ny2*ny2 + nz2*nz2))

    dotN12 = (nx1*nx2+ny1*ny2+nz1*nz2)
    cost = dotN12/sqrt((nx1*nx1+ny1*ny1+nz1*nz1)*(nx2*nx2+ny2*ny2+nz2*nz2))

    data = {'x1': pt1.x, 'y1':pt1.y, 'z1':pt1.z,
            'x2': pt2.x, 'y2':pt2.y, 'z2':pt2.z,
            'x3': pt3.x, 'y3':pt3.y, 'z3':pt3.z,
            'x4': pt4.x, 'y4':pt4.y, 'z4':pt4.z,}

    return [
        [   cost.diff(vx1, vx1)(data), cost.diff(vx1, vy1)(data), cost.diff(vx1, vz1)(data),
            cost.diff(vy1, vx1)(data), cost.diff(vy1, vy1)(data), cost.diff(vy1, vz1)(data),
            cost.diff(vz1, vx1)(data), cost.diff(vz1, vy1)(data), cost.diff(vz1, vz1)(data),],
            
        [   cost.diff(vx1, vx2)(data), cost.diff(vx1, vy2)(data), cost.diff(vx1, vz2)(data),
            cost.diff(vy1, vx2)(data), cost.diff(vy1, vy2)(data), cost.diff(vy1, vz2)(data),
            cost.diff(vz1, vx2)(data), cost.diff(vz1, vy2)(data), cost.diff(vz1, vz2)(data),],

        [   cost.diff(vx1, vx3)(data), cost.diff(vx1, vy3)(data), cost.diff(vx1, vz3)(data),
            cost.diff(vy1, vx3)(data), cost.diff(vy1, vy3)(data), cost.diff(vy1, vz3)(data),
            cost.diff(vz1, vx3)(data), cost.diff(vz1, vy3)(data), cost.diff(vz1, vz3)(data),],
            
        [   cost.diff(vx1, vx4)(data), cost.diff(vx1, vy4)(data), cost.diff(vx1, vz4)(data),
            cost.diff(vy1, vx4)(data), cost.diff(vy1, vy4)(data), cost.diff(vy1, vz4)(data),
            cost.diff(vz1, vx4)(data), cost.diff(vz1, vy4)(data), cost.diff(vz1, vz4)(data),],
        
        [   cost.diff(vx2, vx1)(data), cost.diff(vx2, vy1)(data), cost.diff(vx2, vz1)(data),
            cost.diff(vy2, vx1)(data), cost.diff(vy2, vy1)(data), cost.diff(vy2, vz1)(data),
            cost.diff(vz2, vx1)(data), cost.diff(vz2, vy1)(data), cost.diff(vz2, vz1)(data),],
            
        [   cost.diff(vx2, vx2)(data), cost.diff(vx2, vy2)(data), cost.diff(vx2, vz2)(data),
            cost.diff(vy2, vx2)(data), cost.diff(vy2, vy2)(data), cost.diff(vy2, vz2)(data),
            cost.diff(vz2, vx2)(data), cost.diff(vz2, vy2)(data), cost.diff(vz2, vz2)(data),],
            
        [   cost.diff(vx2, vx3)(data), cost.diff(vx2, vy3)(data), cost.diff(vx2, vz3)(data),
            cost.diff(vy2, vx3)(data), cost.diff(vy2, vy3)(data), cost.diff(vy2, vz3)(data),
            cost.diff(vz2, vx3)(data), cost.diff(vz2, vy3)(data), cost.diff(vz2, vz3)(data),],
            
        [   cost.diff(vx2, vx4)(data), cost.diff(vx2, vy4)(data), cost.diff(vx2, vz4)(data),
            cost.diff(vy2, vx4)(data), cost.diff(vy2, vy4)(data), cost.diff(vy2, vz4)(data),
            cost.diff(vz2, vx4)(data), cost.diff(vz2, vy4)(data), cost.diff(vz2, vz4)(data),],
            
        [   cost.diff(vx3, vx1)(data), cost.diff(vx3, vy1)(data), cost.diff(vx3, vz1)(data),
            cost.diff(vy3, vx1)(data), cost.diff(vy3, vy1)(data), cost.diff(vy3, vz1)(data),
            cost.diff(vz3, vx1)(data), cost.diff(vz3, vy1)(data), cost.diff(vz3, vz1)(data),],
            
        [   cost.diff(vx3, vx2)(data), cost.diff(vx3, vy2)(data), cost.diff(vx3, vz2)(data),
            cost.diff(vy3, vx2)(data), cost.diff(vy3, vy2)(data), cost.diff(vy3, vz2)(data),
            cost.diff(vz3, vx2)(data), cost.diff(vz3, vy2)(data), cost.diff(vz3, vz2)(data),],
            
        [   cost.diff(vx3, vx3)(data), cost.diff(vx3, vy3)(data), cost.diff(vx3, vz3)(data),
            cost.diff(vy3, vx3)(data), cost.diff(vy3, vy3)(data), cost.diff(vy3, vz3)(data),
            cost.diff(vz3, vx3)(data), cost.diff(vz3, vy3)(data), cost.diff(vz3, vz3)(data),],
            
        [   cost.diff(vx3, vx4)(data), cost.diff(vx3, vy4)(data), cost.diff(vx3, vz4)(data),
            cost.diff(vy3, vx4)(data), cost.diff(vy3, vy4)(data), cost.diff(vy3, vz4)(data),
            cost.diff(vz3, vx4)(data), cost.diff(vz3, vy4)(data), cost.diff(vz3, vz4)(data),],
            
        [   cost.diff(vx4, vx1)(data), cost.diff(vx4, vy1)(data), cost.diff(vx4, vz1)(data),
            cost.diff(vy4, vx1)(data), cost.diff(vy4, vy1)(data), cost.diff(vy4, vz1)(data),
            cost.diff(vz4, vx1)(data), cost.diff(vz4, vy1)(data), cost.diff(vz4, vz1)(data),],
            
        [   cost.diff(vx4, vx2)(data), cost.diff(vx4, vy2)(data), cost.diff(vx4, vz2)(data),
            cost.diff(vy4, vx2)(data), cost.diff(vy4, vy2)(data), cost.diff(vy4, vz2)(data),
            cost.diff(vz4, vx2)(data), cost.diff(vz4, vy2)(data), cost.diff(vz4, vz2)(data),],
            
        [   cost.diff(vx4, vx3)(data), cost.diff(vx4, vy3)(data), cost.diff(vx4, vz3)(data),
            cost.diff(vy4, vx3)(data), cost.diff(vy4, vy3)(data), cost.diff(vy4, vz3)(data),
            cost.diff(vz4, vx3)(data), cost.diff(vz4, vy3)(data), cost.diff(vz4, vz3)(data),],
            
        [   cost.diff(vx4, vx4)(data), cost.diff(vx4, vy4)(data), cost.diff(vx4, vz4)(data),
            cost.diff(vy4, vx4)(data), cost.diff(vy4, vy4)(data), cost.diff(vy4, vz4)(data),
            cost.diff(vz4, vx4)(data), cost.diff(vz4, vy4)(data), cost.diff(vz4, vz4)(data),],
    ]
    
class Point:
    pass
    
if __name__ == '__main__':
    pt1=Point();pt2=Point();pt3=Point();pt4=Point();
    pt1.x=-1.056100; pt1.y=-1.088800; pt1.z=-0.039600;
    pt2.x=-0.755800; pt2.y=-0.018400; pt2.z=-0.018500;
    pt3.x= 0.751100; pt3.y= 0.086100; pt3.z= 0.018500;
    pt4.x= 1.060800; pt4.y= 0.643600; pt4.z= 0.923600;
    
    A = hess1(pt1, pt2, pt3, pt4)
    B = hess2(pt1, pt2, pt3, pt4)
    
    A = matrix(A, float64).T
    
    for b in B:
        b = matrix(b, float64).T
        # x = solve(A, b)
        x = lstsq(A, b)
        print matrix(x[0]).T
